unit Obstacle;

interface

uses Classes, Math, Mathematic, Vector, CoordinateSystem, Color, GL, GLu;

type
  TObstacle = class;
  TEdge = array [0..1] of TVector;

  TObstaclesList = class(TList)
  private
  public
    constructor Create;
    constructor CreateCopy(paObstaclesList: TObstaclesList);
    destructor Destroy;override;
    procedure Draw;
  end;

  TObstacle = class
  private
    aID: integer;
    aPosition: TVector;
    aRadius: TFloat;
    aColor: TColor;
    aPrevColor: TColor;
    aType: integer;
    aLogEntity: boolean;
  public
    constructor Create;
    constructor CreateCopy(paObstacle: TObstacle);
    function GetRepulsive(paPedestrian: Pointer; var Distance: TFloat): TVector;virtual;abstract;
    procedure Draw;virtual;abstract;
    destructor Destroy;override;
    property Position: TVector read aPosition write aPosition;
    property Radius: TFloat read aRadius write aRadius;
    property Color: TColor read aColor write aColor;
    property PrevColor: TColor read aPrevColor write aPrevColor;
    property ID: integer read aID write aID;
    property Typ: integer read aType write aType;
    property LogEntity: boolean read aLogEntity write aLogEntity;
  end;

  TCircleObstacle = class(TObstacle)
  private
  public
    constructor Create;
    constructor CreateCopy(paCircleObstacle: TCircleObstacle);
    destructor Destroy;override;
    function GetRepulsive(paPedestrian: Pointer; var Distance: TFloat): TVector;override;
    procedure Draw;override;
  end;

  TPointsList = class(TList)
  private
  public
    constructor Create;
    constructor CreateCopy(paPointsList: TPointsList);
    destructor Destroy;override;
  end;

  TPolygonObstacle = class(TObstacle)
  private
    aPoints: TPointsList;
  public
    constructor Create;
    constructor CreateCopy(paPolygonObstacle: TPolygonObstacle);
    destructor Destroy;override;
    function GetIntersection(paPosition, paVector: TVector; var Distance, Angle: TFloat; paEdge: TEdge): Boolean;
    function GetRepulsive(paPedestrian: Pointer; var Distance: TFloat): TVector;override;
    procedure AddPoint(paPoint: TVector);
    procedure RemovePoint(paPoint: TVector);
    procedure ResetAttributes;
    procedure Draw;override;
    property Points: TPointsList read aPoints write aPoints;
  end;

implementation

uses Pedestrian;

constructor TObstaclesList.Create;
begin
    inherited Create;
end;

constructor TObstaclesList.CreateCopy(paObstaclesList: TObstaclesList);
begin
    inherited Create;
end;

destructor TObstaclesList.Destroy;
var I: Integer;
begin
    inherited Destroy;
end;

procedure TObstaclesList.Draw;
var I: Integer;
begin
    for I := 0 to Count - 1  do begin
        TObstacle(Items[I]).Draw;
    end;
end;

constructor TObstacle.Create;
begin
    inherited Create;
    aPosition := TVector.Create(0.0, 0.0, 0.0);
    aRadius := 0.0;
    aColor := TColor.Create(0.0, 0.0, 0.0, 0.0);
    aPrevColor:= aColor;
    aID:= 0;
    aType:= -1;
    aLogEntity:= false;
end;

constructor TObstacle.CreateCopy(paObstacle: TObstacle);
begin
    inherited Create;
    aPosition := TVector.CreateCopy(aPosition);
    aRadius := paObstacle.aRadius;
    aColor := TColor.CreateCopy(aColor);
    aPrevColor:= TColor.CreateCopy(aColor);
    aType:= -1;
end;

destructor TObstacle.Destroy;
begin
    aPosition.Free;
    aColor.Free;
    aPrevColor.Free;
    inherited Destroy;
end;

constructor TCircleObstacle.Create;
begin
    inherited Create;
end;

constructor TCircleObstacle.CreateCopy(paCircleObstacle: TCircleObstacle);
begin
    inherited Create;
end;

destructor TCircleObstacle.Destroy;
begin
    inherited Destroy;
end;

function TCircleObstacle.GetRepulsive(paPedestrian: Pointer; var Distance: TFloat): TVector;
var Pedestrian: TPedestrianEditor;
    Repulsive, Intrusion: TVector;
begin
    Pedestrian := TPedestrianEditor(paPedestrian);
    Repulsive := TVector.Create;
    Distance := Pedestrian.Position.Distance(aPosition) - aRadius;
    if Intrusion.Length <> 0 then begin
        Intrusion.DivideSelf(Distance);
        Repulsive.MakeCopy(Intrusion);
    end;
    Intrusion.Free;
    Result := Repulsive;
end;

procedure TCircleObstacle.Draw;
var Quad: PGLUquadric;
begin
    Quad := gluNewQuadric;
    glMatrixMode(GL_MODELVIEW);
    glPushMatrix;
    glTranslated(Position.X, Position.Y, Position.Z);
    glColor4d(0.0, 0.0, 0.0, 0.0);
    glLineWidth(1.0);
    glPolygonMode(GL_FRONT_AND_BACK, GL_LINE);
    gluDisk(Quad, aRadius, aRadius, 64, 1);
    glLineWidth(1.0);
    glColor4dv(PGLdouble(aColor.DataPointer));
    glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
    gluDisk(Quad, 0.0, aRadius, 64, 1);
    gluDeleteQuadric(Quad);
    glPopMatrix;
end;

constructor TPointsList.Create;
begin
    inherited Create;
end;

constructor TPointsList.CreateCopy(paPointsList: TPointsList);
begin
    inherited Create;
end;

destructor TPointsList.Destroy;
begin
    inherited Destroy;
end;

constructor TPolygonObstacle.Create;
begin
    inherited Create;
    aPoints := TPointsList.Create;
end;

constructor TPolygonObstacle.CreateCopy(paPolygonObstacle: TPolygonObstacle);
begin
    inherited Create;
    aPoints := TPointsList.CreateCopy(paPolygonObstacle.aPoints);
end;

destructor TPolygonObstacle.Destroy;
var I: Integer;
begin
    for I := 0 to aPoints.Count - 1 do begin
        TVector(aPoints[I]).Free;
    end;
    aPoints.Free;
    inherited Destroy;
end;

function TPolygonObstacle.GetIntersection(paPosition, paVector: TVector; var Distance, Angle: TFloat; paEdge: TEdge): Boolean;
var LocalSpace: TCoordinateSystem;
    Point1, Point2, LocalPoint1, LocalPoint2, NearestPoint1, NearestPoint2: TVector;
    Intersection, MinimalIntersection, Length: TFloat;
    Intersect: Boolean;
    I: Integer;
begin
    Length := paVector.Length;
    Distance := MaxFloat;
    MinimalIntersection := MaxFloat;
    Distance := MaxFloat;
    Intersect := false;
    NearestPoint1 := TVector.Create;
    NearestPoint2 := TVector.Create;
    for I := 0 to aPoints.Count - 1 do begin
        if I = aPoints.Count - 1 then begin
            Point1 := TVector(aPoints[I]);
            Point2 := TVector(aPoints[0]);
        end
        else begin
            Point1 := TVector(aPoints[I]);
            Point2 := TVector(aPoints[I + 1]);
        end;
        LocalSpace := TCoordinateSystem.Create;
        LocalSpace.Setup(paPosition, paVector);
        LocalPoint1 := LocalSpace.LocalizePosition(Point1);
        LocalPoint2 := LocalSpace.LocalizePosition(Point2);
        if ((LocalPoint1.Y < 0) and (LocalPoint2.Y < 0)) or ((LocalPoint1.Y > 0) and (LocalPoint2.Y > 0)) then begin
            LocalPoint1.Free;
            LocalPoint2.Free;
            LocalSpace.Free;
            continue;
        end;
        Intersection := (-LocalPoint1.Y * (LocalPoint2.X - LocalPoint1.X)) / (LocalPoint2.Y - LocalPoint1.Y) + LocalPoint1.X;
        if (Intersection > 0) and (Intersection < Length) and (Intersection < MinimalIntersection) then begin
            Intersect := true;
            MinimalIntersection := Intersection;
            NearestPoint1.MakeCopy(LocalPoint1);
            NearestPoint2.MakeCopy(LocalPoint2);
            if (paEdge[0] <> nil) and (paEdge[1] <> nil) then begin
                paEdge[0].MakeCopy(Point1);
                paEdge[1].MakeCopy(Point2);
            end;
        end;
        LocalPoint1.Free;
        LocalPoint2.Free;
        LocalSpace.Free;
    end;
    if Intersect then begin
        Distance := MinimalIntersection;
        Angle := RadToDeg(ArcTan2(-NearestPoint1.Y, (MinimalIntersection - NearestPoint1.X)));
    end;
    NearestPoint1.Free;
    NearestPoint2.Free;
    Result := Intersect;
end;

function TPolygonObstacle.GetRepulsive(paPedestrian: Pointer; var Distance: TFloat): TVector;
var Pedestrian: TPedestrianEditor;
    RepulsiveForward, RepulsiveRight, RepulsiveLeft, TestVector, Right, Left: TVector;
    NearestEdge: TEdge;
    TempDistance, Angle, ScaleFactor: TFloat;
begin
    Pedestrian := TPedestrianEditor(paPedestrian);

    NearestEdge[0] := TVector.Create;
    NearestEdge[1] := TVector.Create;

    Distance := MaxFloat;
    RepulsiveForward := TVector.Create;
    RepulsiveRight := TVector.Create;
    RepulsiveLeft := TVector.Create;

    TestVector := TVector.CreateCopy(Pedestrian.LocalSpace.Forward);
    TestVector.ScaleSelf(Pedestrian.MaxSpeed / 4.0);
    if GetIntersection(Pedestrian.Position, TestVector, TempDistance, Angle, NearestEdge) then begin
        if TempDistance < Distance then Distance := TempDistance;
        ScaleFactor := Abs(Sin(Angle) * (1.0 / TempDistance));
        RepulsiveForward.X := (NearestEdge[1].Y - NearestEdge[0].Y);
        RepulsiveForward.Y := -(NearestEdge[1].X - NearestEdge[0].X);
        RepulsiveForward.ScaleSelf(ScaleFactor);
    end;
    TestVector.Free;

    Right := TVector.Create(1.0, 1.0, 0.0);
    TestVector := Pedestrian.LocalSpace.GlobalizeDirection(Right);
    TestVector.ScaleSelf(Pedestrian.MaxSpeed / 4.0);
    if GetIntersection(Pedestrian.Position, TestVector, TempDistance, Angle, NearestEdge) then begin
        if TempDistance < Distance then Distance := TempDistance;
        ScaleFactor := Abs(Sin(Angle) * (1.0 / TempDistance));
        RepulsiveRight.X := (NearestEdge[1].Y - NearestEdge[0].Y);
        RepulsiveRight.Y := -(NearestEdge[1].X - NearestEdge[0].X);
        RepulsiveRight.ScaleSelf(ScaleFactor);
    end;
    TestVector.Free;
    Right.Free;

    Left := TVector.Create(1.0, -1.0, 0.0);
    TestVector := Pedestrian.LocalSpace.GlobalizeDirection(Left);
    TestVector.ScaleSelf(Pedestrian.MaxSpeed / 4.0);
    if GetIntersection(Pedestrian.Position, TestVector, TempDistance, Angle, NearestEdge) then begin
        if TempDistance < Distance then Distance := TempDistance;
        ScaleFactor := Abs(Sin(Angle) * (1.0 / Distance));
        RepulsiveLeft.X := (NearestEdge[1].Y - NearestEdge[0].Y);
        RepulsiveLeft.Y := -(NearestEdge[1].X - NearestEdge[0].X);
        RepulsiveLeft.ScaleSelf(ScaleFactor);
    end;
    TestVector.Free;
    Left.Free;

    NearestEdge[0].Free;
    NearestEdge[1].Free;
    Result := RepulsiveForward.AddSelf(RepulsiveRight).AddSelf(RepulsiveLeft);
    RepulsiveRight.Free;
    RepulsiveLeft.Free;
end;

procedure TPolygonObstacle.ResetAttributes;
var Center: TVector;
    MaximalDistance, Distance: TFloat;
    I: Integer;
begin
    Center := TVector.Create;
    for I := 0 to aPoints.Count - 1 do begin
        Center.AddSelf(TVector(aPoints[I]));
    end;
    Center.DivideSelf(aPoints.Count);
    aPosition.MakeCopy(Center);
    Center.Free;
    MaximalDistance := MinFloat;
    for I := 0 to aPoints.Count - 1 do begin
        Distance := Position.Distance(TVector(aPoints[I]));
        if Distance > MaximalDistance then
            MaximalDistance := Distance;
    end;
    aRadius := MaximalDistance;
end;

procedure TPolygonObstacle.AddPoint(paPoint: TVector);
begin
    aPoints.Add(paPoint);
    ResetAttributes;
end;

procedure TPolygonObstacle.RemovePoint(paPoint: TVector);
begin
    aPoints.Remove(paPoint);
    ResetAttributes;
end;

procedure TPolygonObstacle.Draw;
var Tess: PGLUtesselator;
    Quad: PGLUquadric;
    I: Integer;
begin
    glColor4dv(PGLdouble(aColor.DataPointer));
    Tess := gluNewTess;
    glMatrixMode(GL_MODELVIEW);
    glPushMatrix;
    gluTessCallback(Tess, GLU_TESS_BEGIN, @glBegin);
    gluTessCallback(Tess, GLU_TESS_VERTEX, @glVertex3dv);
    gluTessCallback(Tess, GLU_TESS_END, @glEnd);
    gluTessBeginPolygon(Tess, nil);
    gluTessBeginContour(Tess);
    glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
    for I := 0 to aPoints.Count - 1 do begin
        gluTessVertex(Tess, T3dArray(TVector(aPoints[I]).Data), TVector(aPoints[I]).DataPointer);
    end;
    gluTessEndContour(Tess);
    gluTessEndPolygon(Tess);
    gluDeleteTess(Tess);

    glColor4d(0.0, 0.0, 0.0, 0.0);
    glBegin(GL_LINE_LOOP);
    for I := 0 to aPoints.Count - 1 do begin
        glVertex3dv(PGLDouble(TVector(aPoints[I]).DataPointer));
    end;
    glEnd;
    glPopMatrix;

    if Debug then begin
        Quad := gluNewQuadric;
        glMatrixMode(GL_MODELVIEW);
        glPushMatrix;
        glTranslated(aPosition.X, aPosition.Y, aPosition.Z);
        glColor4d(0.0, 0.0, 0.0, 0.0);
        glPolygonMode(GL_FRONT_AND_BACK, GL_LINE);
        gluDisk(Quad, aRadius, aRadius, 64, 1);
        glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
        gluDeleteQuadric(Quad);
        glPopMatrix;
    end;
end;

end.
